// GoalOpTrace.cpp: implementation of the COPTrace class.
//
//////////////////////////////////////////////////////////////////////

#include "StdAfx.h"
#include "GoalOpTrace.h"
#include "DebugDrawContext.h"
#include "Puppet.h"

#include "Navigation/NavigationSystem/NavigationSystem.h"
#include "Navigation/NavigationSystem/OffGridNavigationManager.h"
#include "Navigation/MNM/OffGridLinks.h"

PathFollowResult::TPredictedStates COPTrace::s_tmpPredictedStates;
int COPTrace::s_instanceCount;

//===================================================================
// COPTrace
//===================================================================
COPTrace::COPTrace(bool bExactFollow, float fEndAccuracy, bool bForceReturnPartialPath, bool bStopOnAnimationStart, ETraceEndMode eTraceEndMode) : 
	m_bBlock_ExecuteTrace_untilFullUpdateThenReset(false),
	m_bExactFollow(bExactFollow),
	m_fEndAccuracy(fEndAccuracy),
	m_bForceReturnPartialPath(bForceReturnPartialPath),
	m_stopOnAnimationStart(bStopOnAnimationStart),
	m_Maneuver(eManeuver_None),
	m_ManeuverDir(eManeuverDir_Clockwise),
	m_eTraceEndMode(eTraceEndMode),
	m_ManeuverStartTime(0.f),
	m_landHeight(0.f),
	m_landingDir(ZERO),
	m_landingPos(ZERO),
	m_workingLandingHeightOffset(0.f), 
	m_TimeStep(0.1f),
	m_prevFrameStartTime((int64)(-1000LL)),
	m_fTotalTracingTime(0.f),
	m_lastPosition(ZERO),
  m_fTravelDist(0.f),
	m_inhibitPathRegen(false),
	m_looseAttentionId(0),
	m_bWaitingForPathResult(false),
	m_bWaitingForBusySmartObject(false),
	m_actorTargetRequester(eTATR_None), 
	m_pendingActorTargetRequester(eTATR_None),
	m_passingStraightNavSO(false),
	m_bControlSpeed(true),
	m_accumulatedFailureTime(0.0f),
	m_bEndWithinLookAheadDistance(false)
{
  if (gAIEnv.CVars.DebugPathFinding)
    AILogAlways("COPTrace::COPTrace %p", this);

	++ s_instanceCount;
}

COPTrace::COPTrace(const XmlNodeRef& node) :
	m_bBlock_ExecuteTrace_untilFullUpdateThenReset(false),
	m_bExactFollow(s_xml.GetBool(node, "exactFollow")),
	m_fEndAccuracy(0.f),
	m_bForceReturnPartialPath(s_xml.GetBool(node, "forceReturnPartialPath")),
	m_stopOnAnimationStart(s_xml.GetBool(node, "stopOnAnimationStart")),
	m_Maneuver(eManeuver_None),
	m_ManeuverDir(eManeuverDir_Clockwise),
	m_eTraceEndMode(eTEM_FixedDistance),
	m_ManeuverStartTime(0.f),
	m_landHeight(0.f),
	m_landingDir(ZERO),
	m_landingPos(ZERO),
	m_workingLandingHeightOffset(0.f), 
	m_TimeStep(0.1f),
	m_prevFrameStartTime((int64)(-1000LL)),
	m_fTotalTracingTime(0.f),
	m_lastPosition(ZERO),
	m_fTravelDist(0.f),
	m_inhibitPathRegen(false),
	m_looseAttentionId(0),
	m_bWaitingForPathResult(false),
	m_bWaitingForBusySmartObject(false),
	m_actorTargetRequester(eTATR_None), 
	m_pendingActorTargetRequester(eTATR_None),
	m_passingStraightNavSO(false),
	m_bControlSpeed(true),
	m_accumulatedFailureTime(0.0f),
	m_bEndWithinLookAheadDistance(false)
{
	node->getAttr("endAccuracy", m_fEndAccuracy);

	++ s_instanceCount;
}

COPTrace::COPTrace(const COPTrace& rhs) :
	m_bBlock_ExecuteTrace_untilFullUpdateThenReset(false),
	m_bExactFollow(rhs.m_bExactFollow),
	m_fEndAccuracy(rhs.m_fEndAccuracy),
	m_bForceReturnPartialPath(rhs.m_bForceReturnPartialPath),
	m_stopOnAnimationStart(rhs.m_stopOnAnimationStart),
	m_Maneuver(eManeuver_None),
	m_ManeuverDir(eManeuverDir_Clockwise),
	m_eTraceEndMode(eTEM_FixedDistance),
	m_ManeuverStartTime(0.f),
	m_landHeight(0.f),
	m_landingDir(ZERO),
	m_landingPos(ZERO),
	m_workingLandingHeightOffset(0.f), 
	m_TimeStep(0.1f),
	m_prevFrameStartTime((int64)(-1000LL)),
	m_fTotalTracingTime(0.f),
	m_lastPosition(ZERO),
	m_fTravelDist(0.f),
	m_inhibitPathRegen(false),
	m_looseAttentionId(0),
	m_bWaitingForPathResult(false),
	m_bWaitingForBusySmartObject(false),
	m_actorTargetRequester(eTATR_None), 
	m_pendingActorTargetRequester(eTATR_None),
	m_passingStraightNavSO(false),
	m_bControlSpeed(true),
	m_accumulatedFailureTime(0.0f),
	m_bEndWithinLookAheadDistance(false)
{
	++ s_instanceCount;
}

//===================================================================
// ~COPTrace
//===================================================================
COPTrace::~COPTrace()
{
	CCCPOINT(COPTrace_Destructor);

	CPipeUser * const pPipeUser = m_refPipeUser.GetAIObject();
  if (pPipeUser)
  {
    gAIEnv.pPathfinder->CancelAnyPathsFor(pPipeUser);
		pPipeUser->SetLooseAttentionTarget(NILREF, m_looseAttentionId);
    m_looseAttentionId = 0;

		SOBJECTSTATE& state = pPipeUser->m_State;
		if (!state.continuousMotion)
			state.fDesiredSpeed = 0.f;

    pPipeUser->ClearPath("COPTrace::~COPTrace m_Path");
  }

	m_refPipeUser.Reset();
	m_refNavTarget.Release();

  if (gAIEnv.CVars.DebugPathFinding)
    AILogAlways("COPTrace::~COPTrace %p %s", this, GetNameSafe(pPipeUser));

	-- s_instanceCount;
	if (s_instanceCount == 0)
	{
		stl::free_container(s_tmpPredictedStates);
	}
}


//===================================================================
// Reset
//===================================================================
void COPTrace::Reset(CPipeUser* pPipeUser)
{
	CCCPOINT(COPTrace_Reset);

  if (pPipeUser && gAIEnv.CVars.DebugPathFinding)
    AILogAlways("COPTrace::Reset %s", GetNameSafe(pPipeUser));

  if (pPipeUser)
  {
		CCCPOINT(COPTrace_Reset_A);

    pPipeUser->SetLooseAttentionTarget(NILREF, m_looseAttentionId);
    m_looseAttentionId = 0;
		
		// If we have regenerated the path as part of the periodic regeneration
		// then the request must be cancelled. However, don't always cancel requests
		// because if stick is trying to regenerate the path and we're
		// resetting because we reached the end of the current one, then we'll stop
		// stick from getting its pathfind request.
		// Generally the m_Path.Empty() condition decides between these cases, but
		// Danny todo: not sure if it will always work.
		if (!pPipeUser->m_Path.Empty())
		{
			gAIEnv.pPathfinder->CancelAnyPathsFor(pPipeUser);
		}
		
		pPipeUser->m_nPathDecision = PATHFINDER_ABORT;
    pPipeUser->m_State.fDesiredSpeed = 0.f;
    pPipeUser->ClearPath("COPTrace::Reset m_Path");
  }

	// (MATT) Surely we should reset the stored operand here - but the dummy might best stay, if it exists already {2009/02/18}	
	m_refPipeUser.Reset();
	m_refNavTarget.Release();

  m_fTotalTracingTime = 0.f;
  m_Maneuver = eManeuver_None;
  m_ManeuverStartTime = GetAISystem()->GetFrameStartTime();
  m_landHeight = 0.f;
  m_landingDir.zero();
  m_workingLandingHeightOffset = 0.f;
  m_inhibitPathRegen = false;
  m_bBlock_ExecuteTrace_untilFullUpdateThenReset = false;
	m_actorTargetRequester = eTATR_None;
	m_pendingActorTargetRequester = eTATR_None;
	m_bWaitingForPathResult = false;
	m_bWaitingForBusySmartObject = false;
	m_passingStraightNavSO = false;
	m_fTravelDist = 0;

  m_TimeStep =  0.1f;
  m_prevFrameStartTime = (int64)(-1000LL);
	m_accumulatedFailureTime = 0.0f;

	if (pPipeUser)
	{
		if (m_bExactFollow)
		{
			pPipeUser->m_bLooseAttention = false;
		}
		
		pPipeUser->ClearInvalidatedSOLinks();
	}

	m_bEndWithinLookAheadDistance = false;
}

//===================================================================
// Serialize
//===================================================================
void COPTrace::Serialize(TSerialize ser)
{
	ser.Value("m_bBlock_ExecuteTrace_untilFullUpdateThenReset",m_bBlock_ExecuteTrace_untilFullUpdateThenReset);
	ser.Value("m_ManeuverStartTime",m_ManeuverStartTime);
	ser.Value("m_landHeight",m_landHeight);
	ser.Value("m_workingLandingHeightOffset",m_workingLandingHeightOffset);
	ser.Value("m_landingPos",m_landingPos);
	ser.Value("m_landingDir",m_landingDir);

	ser.Value("m_bExactFollow",m_bExactFollow);
	ser.Value("m_bForceReturnPartialPath",m_bForceReturnPartialPath);
	ser.Value("m_lastPosition",m_lastPosition);
  ser.Value("m_prevFrameStartTime",m_prevFrameStartTime);
  ser.Value("m_fTravelDist",m_fTravelDist);
	ser.Value("m_TimeStep",m_TimeStep);

	m_refPipeUser.Serialize(ser, "m_refPipeUser");

	ser.EnumValue("m_Maneuver", m_Maneuver, eManeuver_None, eManeuver_Forward);
	ser.EnumValue("m_ManeuverDir", m_ManeuverDir, eManeuverDir_Clockwise, eManeuverDir_AntiClockwise);

	ser.Value("m_fTotalTracingTime",m_fTotalTracingTime);
	ser.Value("m_inhibitPathRegen",m_inhibitPathRegen);
	ser.Value("m_fEndAccuracy",m_fEndAccuracy);
	ser.Value("m_looseAttentionId",m_looseAttentionId);

	ser.Value("m_bWaitingForPathResult", m_bWaitingForPathResult);
	ser.Value("m_bWaitingForBusySmartObject", m_bWaitingForBusySmartObject);
	ser.Value("m_passingStraightNavSO", m_passingStraightNavSO);
	ser.EnumValue("m_actorTargetRequester", m_actorTargetRequester, eTATR_None, eTATR_EndOfPath);
	ser.EnumValue("m_pendingActorTargetRequester", m_pendingActorTargetRequester, eTATR_None, eTATR_EndOfPath);
	ser.Value("m_stopOnAnimationStart", m_stopOnAnimationStart);
	ser.Value("m_bEndWithinLookAheadDistance", m_bEndWithinLookAheadDistance);

	m_refNavTarget.Serialize(ser, "m_refNavTarget");
}


//===================================================================
// Execute
//===================================================================
EGoalOpResult COPTrace::Execute(CPipeUser* pPipeUser)
{
	CCCPOINT(COPTrace_Execute);
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

	bool bTraceFinished = ExecuteTrace(pPipeUser, /* full update */ true);

	SOBJECTSTATE& pipeUserState = pPipeUser->m_State;

	if (pPipeUser->m_nPathDecision == PATHFINDER_STILLFINDING)
	{
		if (bTraceFinished)
		{
			pipeUserState.fDesiredSpeed = 0.f;
		}
		return eGOR_IN_PROGRESS;
	}
	else
	{
		if (bTraceFinished)
		{
			// Kevin - Clean up residual data that is causing problems elsewhere
			pipeUserState.fDistanceToPathEnd = 0.f;

			// Done tracing, allow to try to use invalid objects again.
			pPipeUser->ClearInvalidatedSOLinks();
			
			if (pipeUserState.curActorTargetPhase == eATP_Error)
			{
				return eGOR_FAILED;
			}
		}

		if (!m_bEndWithinLookAheadDistance)
		{
			m_bEndWithinLookAheadDistance = (pipeUserState.fDistanceToPathEnd <= pPipeUser->m_movementAbility.pathLookAhead);
			if (m_bEndWithinLookAheadDistance)
			{
				pPipeUser->SetSignal(AISIGNAL_DEFAULT, "OnEndWithinLookAheadDistance");
			}
		}

		return bTraceFinished ? eGOR_SUCCEEDED : eGOR_IN_PROGRESS;
	}
}

//===================================================================
// ExecuteTrace
// this is the same old COPTrace::Execute method
// but now, smart objects should control when it
// should be executed and how its return value
// will be interpreted
//===================================================================
bool COPTrace::ExecuteTrace(CPipeUser* pPipeUser, bool bFullUpdate)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);
  
	// The destructor needs to know the most recent pipe user
	if (m_refPipeUser != pPipeUser)
		m_refPipeUser = GetWeakRef(pPipeUser);

	// HACK: Special case fix for drivers in fall&play
	if (IsVehicleAndDriverIsFallen(pPipeUser))
		return false;	// Trace not finished.

	bool bTraceFinished = false;

	if (m_bWaitingForPathResult)
		if (!HandlePathResult(pPipeUser, &bTraceFinished))
			return bTraceFinished;

  if (m_bBlock_ExecuteTrace_untilFullUpdateThenReset)
  {
    if (bFullUpdate)
    {
			Reset(pPipeUser);
    }
    else
    {
			StopMovement(pPipeUser);
    }
		return true;	// Trace finished
  }

	bool bForceRegeneratePath = false;
	bool bExactPositioning = false;

	// Handle exact positioning and vaSOs.
	if (!HandleAnimationPhase(pPipeUser, bFullUpdate, &bForceRegeneratePath, &bExactPositioning, &bTraceFinished))
		return bTraceFinished;

	// On first frame: [6/4/2010 evgeny]
	if (m_prevFrameStartTime.GetValue() < 0ll)
	{
		// Reset the action input before starting to move.
		// Calling SetAGInput instead of ResetAGInput since we want to reset
		// the input even if some other system has set it to non-idle
		pPipeUser->GetProxy()->SetAGInput(AIAG_ACTION, "idle");

		// Change the SO state to match the movement.
		IEntity* pEntity = pPipeUser->GetEntity();
		IEntity* pNullEntity = 0;
		if (gAIEnv.pSmartObjectManager->SmartObjectEvent("OnMove", pEntity, pNullEntity) != 0)
			return false;	// Trace not finished
	}

	CTimeValue now = GetAISystem()->GetFrameStartTime();

	CIntToFloat timeStepFloat((m_prevFrameStartTime.GetValue() > 0ll)
		? (now - m_prevFrameStartTime).GetMilliSecondsAsInt64()
		: 0);
	m_prevFrameStartTime = now;

	bool isUsing3DNavigation = pPipeUser->IsUsing3DNavigation();

	if (bExactPositioning)
	{
		m_passingStraightNavSO = false;
	}
	else
	{
		IPathFollower* pPathFollower = (gAIEnv.CVars.PredictivePathFollowing && !isUsing3DNavigation) ? pPipeUser->GetPathFollower() : 0;
		float distToSmartObject = pPathFollower ? pPathFollower->GetDistToSmartObject() : pPipeUser->m_Path.GetDistToSmartObject(!isUsing3DNavigation);
		m_passingStraightNavSO = distToSmartObject < 1.f;
  }

	if (bFullUpdate)
	{
		float fDistanceToPathEnd = pPipeUser->m_State.fDistanceToPathEnd;
		const float fExactPosTriggerDistance = 2.5f; //10.0f;

		// Trigger the exact positioning when path is found (sometimes it is still in progress because of previous navSO usage),
		// and when close enough or just finished trace without following it.
		m_bWaitingForBusySmartObject = false;

		if ((fDistanceToPathEnd >= 0.f && fDistanceToPathEnd <= fExactPosTriggerDistance) ||
			pPipeUser->m_Path.GetPathLength(!pPipeUser->IsUsing3DNavigation()) <= fExactPosTriggerDistance)
		{
			TriggerExactPositioning(pPipeUser, &bForceRegeneratePath, &bExactPositioning);
		}
	}

#ifdef _DEBUG
	ExecuteTraceDebugDraw(pPipeUser);
#endif

	float timeStep = max(0.0f, timeStepFloat.Convert() * 0.001f);
	m_fTotalTracingTime += timeStep;
	m_TimeStep = timeStep;

	// If this path was generated with the pathfinder quietly regenerate the path
	// periodically in case something has moved.
	if (bForceRegeneratePath || (bFullUpdate && !m_inhibitPathRegen && !m_passingStraightNavSO && !m_bWaitingForBusySmartObject &&
		pPipeUser->m_movementAbility.pathRegenIntervalDuringTrace > 0.01f &&
		pPipeUser->m_movementAbility.pathRegenIntervalDuringTrace < m_fTotalTracingTime &&
		!pPipeUser->m_Path.GetParams().precalculatedPath &&
		!pPipeUser->m_Path.GetParams().inhibitPathRegeneration))
	{
		if (gAIEnv.CVars.CrowdControlInPathfind != 0 || gAIEnv.CVars.AdjustPathsAroundDynamicObstacles != 0)
		{
			RegeneratePath(pPipeUser, &bForceRegeneratePath);
		}
		
		// The path was forced to be regenerated when a navigational smartobject has been passed.
		if (bForceRegeneratePath)
		{
			m_bWaitingForPathResult = true;
			return false;		// Trace not finished
		}
	}

	//////////////////////////////////////////////////////////////////////////
	// ExecuteTrace Core
	if (!m_bWaitingForPathResult && !m_bWaitingForBusySmartObject)
	{
		if(isUsing3DNavigation)
			bTraceFinished = Execute3D(pPipeUser, bFullUpdate);
		else
		{
			IPathFollower* pPathFollower = gAIEnv.CVars.PredictivePathFollowing ? pPipeUser->GetPathFollower() : 0;
			bTraceFinished = pPathFollower ? ExecutePathFollower(pPipeUser, bFullUpdate, pPathFollower)	: Execute2D(pPipeUser, bFullUpdate);
		}
	}
	//////////////////////////////////////////////////////////////////////////

	if (bExactPositioning)
	{
		// If exact positioning is in motion, do not allow to finish the goal op, just yet.
		// This may happen in some cases when the exact positioning start position is just over
		// the starting point of a navSO, or during the animation playback to keep the
		// prediction warm.

		bTraceFinished = false;
	}

  // prevent future updates unless we get an external reset
	if (bTraceFinished && pPipeUser->m_nPathDecision != PATHFINDER_STILLFINDING)
	{
		if (bFullUpdate)
		{
			Reset(pPipeUser);
			return true;	// Full update, trace finished
		}
		else
		{
			StopMovement(pPipeUser);
			m_bBlock_ExecuteTrace_untilFullUpdateThenReset = true;
			return false;	// Waiting for full update, trace not yet finished at the moment
		}
	}

  return bTraceFinished;
}

//===================================================================
// ExecuteManeuver
//===================================================================
void COPTrace::ExecuteManeuver(CPipeUser* pPipeUser, const Vec3& vSteerDir)
{
  if (fabsf(pPipeUser->m_State.fDesiredSpeed) < 0.001f)
  {
    m_Maneuver = eManeuver_None;
    return;
  }

#ifdef _DEBUG
  // Update the debug movement reason.
  pPipeUser->m_DEBUGmovementReason = CPipeUser::AIMORE_MANEUVER;
#endif

	if (pPipeUser->m_IsSteeringAroundObstacle)
		return;

  float	cosTrh = pPipeUser->m_movementAbility.maneuverTrh;
  if (cosTrh >= 1.f)
    return;

  Vec3 vForwardDir2D = pPipeUser->GetMoveDir();
  if (pPipeUser->m_State.fMovementUrgency < 0.f)
    vForwardDir2D *= -1.f;
  vForwardDir2D.z = 0.f;
  vForwardDir2D.NormalizeSafe();

  Vec3 vSteerDir2D = vSteerDir;
  vSteerDir2D.z = 0.f;
  vSteerDir2D.NormalizeSafe();
  
  float diffCos = vSteerDir2D.Dot(vForwardDir2D);
  // if not maneuvering then require a big angle change to enter it
  if ((diffCos > cosTrh) && (m_Maneuver == eManeuver_None))
    return;

	CTimeValue now = GetAISystem()->GetFrameStartTime();

  // prevent very small wiggles.
  const int maneuverTimeMinLimitMs = 300;
  const int maneuverTimeMaxLimitMs = 7000;
  const int maneuverTimeMs = (int)((m_Maneuver != eManeuver_None) ? (now - m_ManeuverStartTime).GetMilliSecondsAsInt64() : 0);

  // if maneuvering only stop when closely lined up
  static float exitDiffCos = 0.98f;
  if ((diffCos > exitDiffCos) && (m_Maneuver != eManeuver_None) && (maneuverTimeMs > maneuverTimeMinLimitMs))
  {
    m_Maneuver = eManeuver_None;
    return;
  }

	Vec3 myPos = pPipeUser->GetPhysicsPos();

  // hack for instant turning
	CCamera& camera = GetISystem()->GetViewCamera();
  const float minDistSq = square(5.f);
  if ((Distance::Point_PointSq(camera.GetPosition(), myPos) > minDistSq)
		&& !camera.IsSphereVisible_F(Sphere(myPos, pPipeUser->GetRadius())))
  {
    float x = vSteerDir2D.Dot(vForwardDir2D);
    float y = vSteerDir2D.Dot(Vec3(-vForwardDir2D.y, vForwardDir2D.x, 0.f));
    // do it in steps to help physics resolve penetrations...
    float rotAngle = 0.02f * atan2f(y, x);
    Quat q;
		q.SetRotationAA(rotAngle, Vec3Constants<float>::fVec3_OneZ);

    IEntity* pEntity = pPipeUser->GetEntity();
    Quat qCur = pEntity->GetRotation();
    pEntity->SetRotation(q * qCur);
    m_Maneuver = eManeuver_None;
    return;
  }

	// First figure out which direction to rotate (normally const over the whole maneuver).
	// Then figure out if it's forwards or backwards.
	// Then steer based on the combination of these two.

  bool bMovingFwd = vForwardDir2D.Dot(pPipeUser->GetVelocity()) > 0.f;

  // start a new maneuver?
  if (m_Maneuver == eManeuver_None)
  {
		m_Maneuver = eManeuver_Backward;
		// set the direction
		Vec3 dirCross = vForwardDir2D.Cross(vSteerDir2D);
		m_ManeuverDir = dirCross.z > 0.f ? eManeuverDir_AntiClockwise : eManeuverDir_Clockwise;
    m_ManeuverStartTime = GetAISystem()->GetFrameStartTime();
  }
  else
  {
		if (maneuverTimeMs > maneuverTimeMaxLimitMs)
    {
      m_Maneuver = m_Maneuver == eManeuver_Forward ? eManeuver_Backward : eManeuver_Forward;
      m_ManeuverStartTime = now;
    }
		else
		{
			if (m_Maneuver == eManeuver_Backward)
			{
				if ( fabsf(vSteerDir2D.Dot(vForwardDir2D)) < cosf(DEG2RAD(95.f)))
				{
					m_Maneuver = eManeuver_Forward;
					m_ManeuverStartTime = now;
				}
			}
			else
			{
				if (fabsf(vSteerDir2D.Dot(vForwardDir2D)) > cosf(DEG2RAD(2.f)))
				{
					m_Maneuver = eManeuver_Backward;
					m_ManeuverStartTime = now;
				}
			}
		}
  }

  // now turn these into actual requests
	
	if (m_Maneuver == eManeuver_Backward)
	{
		pPipeUser->m_State.fDesiredSpeed = -5.f;
	}
	else
	{
		float normalSpeed, minSpeed, maxSpeed;
		pPipeUser->GetMovementSpeedRange(AISPEED_WALK, false, normalSpeed, minSpeed, maxSpeed);
		pPipeUser->m_State.fDesiredSpeed = minSpeed;
	}

  Vec3 leftDir(-vForwardDir2D.y, vForwardDir2D.x, 0.f);
  pPipeUser->m_State.vMoveDir = ((m_ManeuverDir == eManeuverDir_Clockwise) ^ (pPipeUser->m_State.fDesiredSpeed >= 0.f))
		? leftDir
		: -leftDir;

	/*
	// Debug Draw
	ColorB color;
	if (m_ManeuverDir == eManeuverDir_AntiClockwise)
	{
		if (m_Maneuver == eManeuver_Backward)
		{
			color.Set(0, 255, 0);
		}
		else
		{
			color.Set(255, 255, 255);
		}
	}
	else
	{
		if (m_Maneuver == eManeuver_Backward)
		{
			color.Set(255, 0, 0);
		}
		else
		{
			color.Set(0, 0, 255);
		}
	}
	color.a = 255;
	CDebugDrawContext dc;
	dc->DrawArrow(myPos, 10.f * pPipeUser->m_State.vMoveDir, 2.f, color);
	dc->Draw2dLabel(50.f, 50.f, 2.f, ColorB(255, 255, 255), false, "Desired Speed: %f, Maneuver dir: %d",
		pPipeUser->m_State.fDesiredSpeed,
		m_ManeuverDir);
	dc->DrawArrow(myPos, 10.f * vSteerDir, 1.f, ColorB(255, 255, 0));
	dc->DrawArrow(myPos, 10.f * vForwardDir2D, .5f, ColorB(255, 0, 255));
	dc->DrawArrow(myPos, 15.f * pPipeUser->GetVelocity(), .2f, ColorB(0, 255, 255));
	*/
}

//===================================================================
// ExecutePreamble
//===================================================================
bool COPTrace::CheckIfPathIsEmpty(CPipeUser* pPipeUser)
{
	CCCPOINT(COPTrace_ExecutePreamble);
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

	if (m_lastPosition.IsZero())
    m_lastPosition = pPipeUser->GetPhysicsPos();

	if (!m_refNavTarget.GetAIObject())
	{
		if (pPipeUser->m_Path.Empty())
		{
	    pPipeUser->m_State.fDesiredSpeed = 0.f;
			m_inhibitPathRegen = true;
		}
		else
		{
			// Obtain a NavTarget
			stack_string name("navTarget_");
			name += GetNameSafe(pPipeUser);
			
			gAIEnv.pAIObjectManager->CreateDummyObject(m_refNavTarget, name, CAIObject::STP_REFPOINT);
			if (CAIObject* pNavTarget = m_refNavTarget.GetAIObject())
			{
				pNavTarget->SetPos(pPipeUser->GetPhysicsPos());
			}
			
			m_inhibitPathRegen = false;
		}
	}
	else
	{
		m_inhibitPathRegen = false;
	}

	return m_inhibitPathRegen;
}

//===================================================================
// ExecutePostamble
//===================================================================
bool COPTrace::ExecutePostamble(CPipeUser* pPipeUser, bool reachedEnd, bool fullUpdate, bool b2D)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

  // do this after maneuver since that needs to track how far we moved
  Vec3 opPos = pPipeUser->GetPhysicsPos();
  m_fTravelDist += b2D ? Distance::Point_Point2D(opPos, m_lastPosition) : Distance::Point_Point(opPos, m_lastPosition);
  m_lastPosition = opPos;

	SOBJECTSTATE& pipeUserState = pPipeUser->m_State;

  // only consider trace to be done once the agent has stopped.
  if (reachedEnd && m_fEndAccuracy >= 0.f)
  {
    Vec3 vel = pPipeUser->GetVelocity();
    vel.z = 0.f;
    float speed = vel.Dot(pipeUserState.vMoveDir);
    const float criticalSpeed = 0.01f;
    if (speed > criticalSpeed)
    {

      if (gAIEnv.CVars.DebugPathFinding)
        AILogAlways("COPTrace reached end but waiting for speed %5.2f to fall below %5.2f %s", 
					speed, criticalSpeed, GetNameSafe(pPipeUser));

      reachedEnd = false;
      pipeUserState.fDesiredSpeed = 0.f;
      m_inhibitPathRegen = true;
    }
  }

  // only consider trace to be done once the agent has stopped.
  if (reachedEnd)
  {
    pipeUserState.fDesiredSpeed = 0.f;
    m_inhibitPathRegen = true;
    return true;	// Trace finished
  }

  if (fullUpdate)
  {
    const float forbiddenTolerance = 1.5f;
		
		GraphNode* pThisNode = gAIEnv.pGraph->GetNodeManager().GetNode(pPipeUser->GetNavNodeIndex());
		
    pPipeUser->m_bLastNearForbiddenEdge = pThisNode && 
      (pThisNode->navType & (IAISystem::NAV_TRIANGULAR | IAISystem::NAV_ROAD)) && 
      gAIEnv.pNavigation->IsPointOnForbiddenEdge(opPos, forbiddenTolerance, 0, 0, false);
  }

	/*
  // code below here checks/handles dynamic objects
  if (pPipeUser->m_Path.GetParams().precalculatedPath)
    return false;
	*/

  //FIXME: Investigate if this is necessary anymore with new smart path follower
  const bool doSteering = (gAIEnv.configuration.eCompatibilityMode != ECCM_CRYSIS2);
  if (doSteering && !pPipeUser->m_bLastNearForbiddenEdge)
  {
		pPipeUser->m_IsSteeringAroundObstacle = pPipeUser->NavigateAroundObjects(opPos, fullUpdate);
		if (pPipeUser->m_IsSteeringAroundObstacle)
		{
			stl::free_container(pipeUserState.predictedCharacterStates.states);
		}
  }

  return false;
}

//===================================================================
// ExecutePathFollower
//===================================================================
bool COPTrace::ExecutePathFollower(CPipeUser* pPipeUser, bool fullUpdate, IPathFollower* pPathFollower)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

	AIAssert(pPathFollower);
	if (!pPathFollower)
		return true;

	if (m_TimeStep <= 0.f)
		return false;

  if (CheckIfPathIsEmpty(pPipeUser))
	{
		// Path empty, nav target nil, path regeneration inhibited, trace finished [6/5/2010 evgeny]
		return true;
	}

	CCCPOINT(COPTrace_ExecutePathFollower);

	SOBJECTSTATE& pipeUserState = pPipeUser->m_State;

	float fNormalSpeed, fMinSpeed, fMaxSpeed;
	pPipeUser->GetMovementSpeedRange(pipeUserState.fMovementUrgency, pipeUserState.allowStrafing,
		fNormalSpeed, fMinSpeed, fMaxSpeed);

  // Update the path follower - really these things shouldn't change
	// TODO: Leaving const_cast here it indicate how awful this is!
  PathFollowerParams& params = const_cast<PathFollowerParams&>(pPathFollower->GetParams());
	params.minSpeed = fMinSpeed;
	params.maxSpeed = fMaxSpeed;
	params.normalSpeed = clamp(fNormalSpeed, params.minSpeed, params.maxSpeed);

  params.endDistance = 0.f;

	bool bContinueMovingAtEnd = pPipeUser->m_Path.GetParams().continueMovingAtEnd;

	// [Mikko] Slight hack to make the formation following better.
	// When following formation, the agent is often very close to the formation.
	// This code forces the actor to lag a bit behind, which helps to follow the formation much more smoothly.
	// TODO: Revive approach-at-distance without cutting the path and add that as extra parameter for stick.
	if (bContinueMovingAtEnd && m_pendingActorTargetRequester == eTATR_None)
	{
		CAIObject* pPathFindTarget = pPipeUser->m_refPathFindTarget.GetAIObject();
		if (pPathFindTarget && (pPathFindTarget->GetSubType() == IAIObject::STP_FORMATION))
		{
			params.endDistance = 1.f;
		}
	}

  params.maxAccel = pPipeUser->m_movementAbility.maxAccel;
  params.maxDecel = pPipeUser->m_movementAbility.maxDecel;
  params.stopAtEnd = !bContinueMovingAtEnd;

  PathFollowResult result;

  PathFollowResult::TPredictedStates& predictedStates = s_tmpPredictedStates;

	// Lower the prediction calculations for puppets which have lo update priority (means invisible or distant).
	bool highPriority;
	if (CPuppet* pPuppet = pPipeUser->CastToCPuppet())
	{
		EPuppetUpdatePriority ePuppetUpdatePriority = pPuppet->GetUpdatePriority();
		highPriority = (ePuppetUpdatePriority == AIPUP_VERY_HIGH) || (ePuppetUpdatePriority == AIPUP_HIGH);
	}
	else
	{
		highPriority = true;
	}
	
	const float PREDICTION_DELTA_TIME = 0.1f;
	const float PREDICTION_TIME = 1.f;

	if (highPriority)
	{
		result.desiredPredictionTime = PREDICTION_TIME;
		predictedStates.resize(size_t(PREDICTION_TIME / PREDICTION_DELTA_TIME + 0.5f));
		result.predictedStates = &predictedStates;
	}
	else
	{
		result.desiredPredictionTime = 0.f;
		predictedStates.clear();
		result.predictedStates =  0;
	}
	
  result.predictionDeltaTime = PREDICTION_DELTA_TIME;

	Vec3 curPos = pPipeUser->GetPhysicsPos();

	// If there's an animation in progress (typically SO playing)
	if (m_actorTargetRequester == eTATR_NavSO)
	{
		switch (pipeUserState.curActorTargetPhase)
		{
		case eATP_Playing:
		case eATP_Finished:
		case eATP_StartedAndFinished:
			// Path follow from the starting position of the path
			assert(!pipeUserState.curActorTargetFinishPos.IsZero());
			curPos = pipeUserState.curActorTargetFinishPos;
			//pPipeUser->m_Path.GetPosAlongPath(curPos, 0.0f);
		}
	}
	
	const Vec3 curVel = pPipeUser->GetVelocity();

	const bool targetReachable = pPathFollower->Update(result, curPos, curVel, m_TimeStep);
	if (targetReachable)
	{
		Vec3 desiredMoveDir = result.velocityOut;
		float desiredSpeed = desiredMoveDir.NormalizeSafe();

		pipeUserState.fDesiredSpeed = desiredSpeed;
		pipeUserState.vMoveDir = desiredMoveDir;
		pipeUserState.fDistanceToPathEnd = pPathFollower->GetDistToEnd(&curPos);

		int num = min<int>(predictedStates.size(), SAIPredictedCharacterStates::maxStates);
		pipeUserState.predictedCharacterStates.states.resize(num);
		for (int i = 0; i < num; ++i)
		{
			const PathFollowResult::SPredictedState& state = predictedStates[i];
			pipeUserState.predictedCharacterStates.states[i].Set(state.pos, state.vel, (1 + i) * PREDICTION_DELTA_TIME);
		}

		// Precision movement support: A replacement for the old velocity based movement system above.
		// NOTE: If the move target and inflection point are equal (and non-zero) they mark the end of the path.
		pipeUserState.vMoveTarget = result.followTargetPos;
		pipeUserState.vInflectionPoint = result.inflectionPoint;

		return ExecutePostamble(pPipeUser, result.reachedEnd, fullUpdate, params.use2D);
	}
	else
	{
		m_accumulatedFailureTime += gEnv->pTimer->GetFrameTime();

		if (m_accumulatedFailureTime > 0.5f)
		{
			m_accumulatedFailureTime = 0.0f;

			bool forceRegeneratePath = true;
			RegeneratePath(pPipeUser, &forceRegeneratePath);
			m_bWaitingForPathResult = true;
		}

		return StillTracing;
	}
}

//====================================================================
// Execute2D
//====================================================================
bool COPTrace::Execute2D(CPipeUser* pPipeUser, bool fullUpdate)
{
  if (CheckIfPathIsEmpty(pPipeUser))
	{
		// Path empty, nav target nil, path regeneration inhibited, trace finished [7/31/2012 evgeny]
    return true;
	}

	SOBJECTSTATE& state = pPipeUser->m_State;
	CNavPath& path = pPipeUser->m_Path;
	const AgentMovementAbility& movementAbility = pPipeUser->m_movementAbility;

  // output
  Vec3 steerDir(ZERO);
  float distToEnd = 0.f;
  float distToPath = 0.f;
	bool isResolvingSticking = false;
  Vec3 pathDir(ZERO);
  Vec3 pathAheadDir(ZERO);
  Vec3 pathAheadPos(ZERO);

	// input
	const Vec3 opPos = pPipeUser->GetPhysicsPos();
	Vec3 fwdDir = pPipeUser->GetMoveDir();
	if (state.fMovementUrgency < 0.0f)
	{
		fwdDir *= -1.f;
	}
	pe_status_dynamics dSt;
	pPipeUser->GetPhysics()->GetStatus(&dSt);
	Vec3 opVel = (m_Maneuver == eManeuver_None) ? dSt.v : fwdDir * 5.f;
	float fLookAhead = movementAbility.pathLookAhead;
	float pathRadius = movementAbility.pathRadius;

	/*
	CDebugDrawContext dc;
	dc->DrawLine(opPos, ColorB(255, 0, 0), opPos + dSt.v, ColorB(255, 0, 0));
	dc->Draw3dLabel(opPos + dSt.v, 2.f, "dSt.v");
	dc->DrawLine(opPos, ColorB(0, 255, 0), opPos + pPipeUser->GetMoveDir() * 5.f, ColorB(0, 255, 0));
	dc->Draw3dLabel(opPos + pPipeUser->GetMoveDir() * 5.f, 2.f, "vMoveDir");
	dc->DrawLine(opPos, ColorB(0, 0, 255), opPos + fwdDir * 8.f, ColorB(0, 0, 255));
	dc->Draw3dLabel(opPos + fwdDir * 8.f, 2.f, "fwdDir");
	*/

  bool bNoPathSegmentsLeft = !path.UpdateAndSteerAlongPath(
    steerDir,
		distToEnd,
		distToPath,
		isResolvingSticking,
    pathDir,
		pathAheadDir,
		pathAheadPos,

    opPos,
		opVel,
		fLookAhead,
		pathRadius,
		m_TimeStep,
		movementAbility.resolveStickingInTrace,
		true /* 2D */
	);

  state.fDistanceToPathEnd = max(0.f, distToEnd);
	state.fDistanceOffFromPath = path.GetDistBetweenCurrentPositionAndPoint(opPos);

#ifdef _DEBUG
  // Update the debug movement reason.
  pPipeUser->m_DEBUGmovementReason = CPipeUser::AIMORE_TRACE;
#endif

  distToEnd -= /*m_fEndDistance*/ -path.GetDiscardedPathLength();

	if (bNoPathSegmentsLeft || (distToEnd <= .1f))
	{
		return ExecutePostamble(pPipeUser, true /* reached end */, fullUpdate, true /* 2D */);
	}


	//////////////////////////////////////////////////////////////////////////

  if (m_refNavTarget)
	{
		Vec3 targetPos;
		if (path.GetPosAlongPath(targetPos, fLookAhead, true /* 3D */, true /* extrapolate beyond end */))
		{
			m_refNavTarget->SetPos(targetPos);
		}
	}

  //turning maneuvering 
  //FIXME: Investigate if this is necessary anymore with new smart path follower
  if (gAIEnv.configuration.eCompatibilityMode != ECCM_CRYSIS2)
	{
    ExecuteManeuver(pPipeUser, steerDir);

		if (m_Maneuver != eManeuver_None)
		{
			m_fTravelDist += Distance::Point_Point2D(opPos, m_lastPosition);
			m_lastPosition = opPos;
			
			// prevent path regeneration
			m_fTotalTracingTime = 0.f;
			return false;
		}
	}

  // These will be adjusted to the range 0-1 to select a speed between slow and normal
  float dirSpeedMod = 1.f;
  float curveSpeedMod = 1.f;
  float endSpeedMod = 1.f;
  float slopeMod = 1.f;

  // Speed falloff - slow down if current direction is too different from desired
  if ((pPipeUser->GetType() == AIOBJECT_VEHICLE) && (movementAbility.velDecay > 0.f))
  {
		pathAheadDir.z = 0.f;
		pathAheadDir.NormalizeSafe();
		
		const float coeff = 1.f; // if this is > 1 then there is a "window" where no slow-down occurs
    float	dot = coeff * pathAheadDir.Dot(fwdDir);

    if (dot < 1.f)
		{
      dirSpeedMod = dot / ((1.f - dot) * movementAbility.velDecay);
		}
  }

	Vec3 steerDir2D = steerDir;
	steerDir2D.z = 0.f;
	steerDir2D.NormalizeSafe();

  // slow down due to the path curvature
  float lookAheadForSpeedControl;
  if (movementAbility.pathSpeedLookAheadPerSpeed < 0.f)
    lookAheadForSpeedControl = -movementAbility.pathSpeedLookAheadPerSpeed * fLookAhead * state.fMovementUrgency;
  else
    lookAheadForSpeedControl = movementAbility.pathSpeedLookAheadPerSpeed * pPipeUser->GetVelocity().GetLength();

  if (lookAheadForSpeedControl > 0.f)
  {
    Vec3 pos, dir;
    float lowestPathDot = 0.f;
    bool curveOK = path.GetPathPropertiesAhead(lookAheadForSpeedControl, true /* 2D */, pos, dir, NULL, lowestPathDot, true);

    Vec3 thisPathSegDir = (path.GetNextPathPoint()->vPos - path.GetPrevPathPoint()->vPos);
    thisPathSegDir.z = 0.0f;
    thisPathSegDir.NormalizeSafe();
    float thisDot = thisPathSegDir.Dot(steerDir2D);
    if (thisDot < lowestPathDot)
		{
      lowestPathDot = thisDot;
		}

    if (curveOK)
    {
      float a = 1.f - 2.f * movementAbility.cornerSlowDown; // decrease this to make the speed drop off quicker with angle
      float b = 1.f - a;
      curveSpeedMod = a + b * lowestPathDot;
    }

    // slow down at end
    if ((m_fEndAccuracy >= 0.f) && (m_eTraceEndMode != eTEM_MinimumDistance))
    {
      const float slowDownDistScale = 2.f;
      const float minEndSpeedMod = 0.1f;
      float slowDownDist = slowDownDistScale * lookAheadForSpeedControl;
      float workingDistToEnd = m_fEndAccuracy + distToEnd - 0.2f * lookAheadForSpeedControl;
      if (slowDownDist > 0.1f && workingDistToEnd < slowDownDist)
      {
        endSpeedMod = workingDistToEnd / slowDownDist;
        Limit(endSpeedMod, minEndSpeedMod, 1.f);
      }
    }
  }

	float slopeModCoeff = movementAbility.slopeSlowDown;
  // slow down when going down steep slopes (especially stairs!)
	int buildingID = -1;
	if ((slopeModCoeff > 0) && 
		(gAIEnv.pNavigation->CheckNavigationType(pPipeUser->GetPos(), buildingID,
		pPipeUser->GetMovementAbility().pathfindingProperties.navCapMask) == IAISystem::NAV_WAYPOINT_HUMAN))
  {
    static float slowDownSlope = 0.5f;
    float pathHorDist = steerDir.GetLength2D();
    if (pathHorDist > 0.0f && steerDir.z < 0.0f)
    {
			float slope = -steerDir.z / pathHorDist * slopeModCoeff;
      slopeMod = 1.0f - slope / slowDownSlope;
      static float minSlopeMod = 0.5f;
      Limit(slopeMod, minSlopeMod, 1.0f);
    }
  }

  // slow down when going up steep slopes
	if(slopeModCoeff > 0)
  {
    IPhysicalEntity * pPhysics = pPipeUser->GetPhysics();
    pe_status_living status;
    int valid = pPhysics->GetStatus(&status);
		if (valid)
		{
			if (!status.bFlying)
			{
				Vec3 sideDir(-steerDir2D.y, steerDir2D.x, 0.0f);
				Vec3 slopeN = status.groundSlope - status.groundSlope.Dot(sideDir) * sideDir;
				slopeN.NormalizeSafe();
				// d is -ve for uphill
				float d = steerDir2D.Dot(status.groundSlope);
				Limit(d, -0.99f, 0.99f);
				// map d=-1 -> -inf, d=0 -> 1, d=1 -> inf
					float uphillSlopeMod = (1 + d / (1.0f - square(d)))*slopeModCoeff;
				static float minUphillSlopeMod = 0.5f;
				if (uphillSlopeMod < minUphillSlopeMod)
					uphillSlopeMod = minUphillSlopeMod;
				if (uphillSlopeMod < 1.0f)
					slopeMod = min(slopeMod, uphillSlopeMod);
			}
		}
	}

	float normalSpeed, minSpeed, maxSpeed;
	pPipeUser->GetMovementSpeedRange(state.fMovementUrgency, state.allowStrafing, normalSpeed, minSpeed, maxSpeed);

	if ((pPipeUser->GetSubType() == IAIObject::STP_CAR) && (slopeModCoeff > 0.f) && !opVel.IsZero())
	{
		float degSlope1 = RAD2DEG(asin_tpl(fwdDir.z));
		float degSlope2 = RAD2DEG(asin_tpl(pathAheadDir.z));
		float degDeltaSlope = degSlope1 - degSlope2;
		float speed = opVel.GetLength();

		float slopeCoeff = slopeModCoeff * degDeltaSlope * speed;
		if ((slopeCoeff >= 1.f) && (speed > normalSpeed / 5.f))
		{
			slopeMod = 0.f;
		}
	}

  float maxMod = min(min(min(dirSpeedMod, curveSpeedMod), endSpeedMod), slopeMod);
  Limit(maxMod, 0.0f, 1.0f);
  //    maxMod = pow(maxMod, 2.0f);
		
	float newDesiredSpeed = (1.f - maxMod) * minSpeed + maxMod * normalSpeed;

	if (m_bControlSpeed)
	{

		float change = newDesiredSpeed - state.fDesiredSpeed;
		if (change > m_TimeStep * movementAbility.maxAccel)
			change = m_TimeStep * movementAbility.maxAccel;
		else if (change < -m_TimeStep * movementAbility.maxDecel)
			change = -m_TimeStep * movementAbility.maxDecel;
		state.fDesiredSpeed += change;
	}
	else
	{
		state.fDesiredSpeed = newDesiredSpeed;
	}

  state.vMoveDir = steerDir2D;
  if (state.fMovementUrgency < 0.f)
	{
    state.vMoveDir *= -1.f;
	}

  // prediction
//    static bool doPrediction = true;
	stl::free_container(state.predictedCharacterStates.states);

  return ExecutePostamble(pPipeUser, false /* end not reached */, fullUpdate, true /* 2D */);
}

//====================================================================
// Execute3D
//====================================================================
bool COPTrace::Execute3D(CPipeUser* pPipeUser, bool fullUpdate)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

  if (CheckIfPathIsEmpty(pPipeUser))
	{
		// Path empty, nav target nil, path regeneration inhibited, trace finished [7/31/2012 evgeny]
		return true;
	}

  // Ideally do this check less... but beware we might regen the path and the end might change a bit(?)
  if (fullUpdate)
  {
    if (pPipeUser->GetType() == AIOBJECT_VEHICLE && m_fEndAccuracy == 0.0f && !pPipeUser->m_Path.GetPath().empty())
    {
      Vec3 endPt = pPipeUser->m_Path.GetPath().back().vPos;
      // ideally we would use AICE_ALL here, but that can result in intersection with the heli when it
      // gets to the end of the path...
      bool gotFloor = GetFloorPos(m_landingPos, endPt, 0.5f, 1.0f, 1.0f, AICE_STATIC);
      if (gotFloor)
        m_landHeight = 2.0f;
      else
        m_landHeight = 0.0f;
      if (m_workingLandingHeightOffset > 0.0f)
        m_inhibitPathRegen = true;
    }
    else
    {
      m_landHeight = 0.0f;
      m_inhibitPathRegen = false;
    }
  }

  // input
  Vec3 opPos = pPipeUser->GetPhysicsPos();
  Vec3 fakeOpPos = opPos;
  fakeOpPos.z -= m_workingLandingHeightOffset;
  if (pPipeUser->m_IsSteeringAroundObstacle)
    fakeOpPos.z -= pPipeUser->m_flightSteeringZOffset;

  pe_status_dynamics  dSt;
  pPipeUser->GetPhysics()->GetStatus(&dSt);
  Vec3 opVel = dSt.v;
  float lookAhead = pPipeUser->m_movementAbility.pathLookAhead;
  float pathRadius = pPipeUser->m_movementAbility.pathRadius;
  bool resolveSticking = pPipeUser->m_movementAbility.resolveStickingInTrace;

  // output
  Vec3	steerDir(ZERO);
  float distToEnd = 0.0f;
  float distToPath = 0.0f;
  Vec3 pathDir(ZERO);
  Vec3 pathAheadDir(ZERO);
  Vec3 pathAheadPos(ZERO);

  bool isResolvingSticking = false;

  bool stillTracingPath = pPipeUser->m_Path.UpdateAndSteerAlongPath(steerDir, distToEnd, distToPath, isResolvingSticking,
    pathDir, pathAheadDir, pathAheadPos,
    fakeOpPos, opVel, lookAhead, pathRadius, m_TimeStep, resolveSticking, false);
  pPipeUser->m_State.fDistanceToPathEnd = max(0.0f, distToEnd);

#ifdef _DEBUG
  // Update the debug movement reason.
  pPipeUser->m_DEBUGmovementReason = CPipeUser::AIMORE_TRACE;
#endif

  distToEnd -= distToPath;
  distToEnd -= m_landHeight * 2.0f;
  if (distToEnd < 0.0f)
    stillTracingPath = false;

  if (!stillTracingPath && m_landHeight > 0.0f)
  {
    return ExecuteLanding(pPipeUser, m_landingPos);
  }

  distToEnd -= /*m_fEndDistance*/ -pPipeUser->m_Path.GetDiscardedPathLength();
  bool reachedEnd = !stillTracingPath;
  if (stillTracingPath && distToEnd > 0.0f)
  {
    Vec3 targetPos;
    if (m_refNavTarget && pPipeUser->m_Path.GetPosAlongPath(targetPos, lookAhead, true, true))
      m_refNavTarget->SetPos(targetPos);

		//    float normalSpeed = pPipeUser->GetNormalMovementSpeed(pPipeUser->m_State.fMovementUrgency, true);
		//    float slowSpeed = pPipeUser->GetManeuverMovementSpeed();
		//    if (slowSpeed > normalSpeed) slowSpeed = normalSpeed;
		float normalSpeed, minSpeed, maxSpeed;
		pPipeUser->GetMovementSpeedRange(pPipeUser->m_State.fMovementUrgency, pPipeUser->m_State.allowStrafing, normalSpeed, minSpeed, maxSpeed);

    // These will be adjusted to the range 0-1 to select a speed between slow and normal
    float dirSpeedMod = 1.0f;
    float curveSpeedMod = 1.0f;
    float endSpeedMod = 1.0f;
    float moveDirMod = 1.0f;

    // slow down due to the path curvature
    float lookAheadForSpeedControl;
    if (pPipeUser->m_movementAbility.pathSpeedLookAheadPerSpeed < 0.0f)
      lookAheadForSpeedControl = lookAhead * pPipeUser->m_State.fMovementUrgency;
    else
      lookAheadForSpeedControl = pPipeUser->m_movementAbility.pathSpeedLookAheadPerSpeed * pPipeUser->GetVelocity().GetLength();

    lookAheadForSpeedControl -= distToPath;
    if (lookAheadForSpeedControl < 0.0f)
      lookAheadForSpeedControl = 0.0f;

    if (lookAheadForSpeedControl > 0.0f)
    {
      Vec3 pos, dir;
      float lowestPathDot = 0.0f;
      bool curveOK = pPipeUser->m_Path.GetPathPropertiesAhead(lookAheadForSpeedControl, true, pos, dir, 0, lowestPathDot, true);
      Vec3 thisPathSegDir = (pPipeUser->m_Path.GetNextPathPoint()->vPos - pPipeUser->m_Path.GetPrevPathPoint()->vPos).GetNormalizedSafe();
      float thisDot = thisPathSegDir.Dot(steerDir);
      if (thisDot < lowestPathDot)
        lowestPathDot = thisDot;
      if (curveOK)
      {
        float a = 1.0f - 2.0f * pPipeUser->m_movementAbility.cornerSlowDown; // decrease this to make the speed drop off quicker with angle
        float b = 1.0f - a;
        curveSpeedMod = a + b * lowestPathDot;
      }
    }

    // slow down at end 
    if (m_fEndAccuracy >= 0.0f)
    {
      static float slowDownDistScale = 1.0f;
      float slowDownDist = slowDownDistScale * lookAheadForSpeedControl;
      float workingDistToEnd = m_fEndAccuracy + distToEnd - 0.2f * lookAheadForSpeedControl;
      if (slowDownDist > 0.1f && workingDistToEnd < slowDownDist)
      {
        // slow speeds are for manouevering - here we want something that will actually be almost stationary
				minSpeed *= 0.1f;
        endSpeedMod = workingDistToEnd / slowDownDist;
        Limit(endSpeedMod, 0.0f, 1.0f);
        m_workingLandingHeightOffset = (1.0f - endSpeedMod) * m_landHeight;
      }
      else
      {
        m_workingLandingHeightOffset = 0.0f;
      }
    }

    float maxMod = min(min(min(dirSpeedMod, curveSpeedMod), endSpeedMod), moveDirMod);
    Limit(maxMod, 0.0f, 1.0f);

		float newDesiredSpeed = (1.0f - maxMod) * minSpeed + maxMod * normalSpeed;
    float change = newDesiredSpeed - pPipeUser->m_State.fDesiredSpeed;
    if (change > m_TimeStep * pPipeUser->m_movementAbility.maxAccel)
      change = m_TimeStep * pPipeUser->m_movementAbility.maxAccel;
		else if (change < -m_TimeStep * pPipeUser->m_movementAbility.maxDecel)
			change = -m_TimeStep * pPipeUser->m_movementAbility.maxDecel;
    pPipeUser->m_State.fDesiredSpeed += change;

    pPipeUser->m_State.vMoveDir = steerDir;
    if (pPipeUser->m_State.fMovementUrgency < 0.0f)
      pPipeUser->m_State.vMoveDir *= -1.0f;

    // prediction
//    static bool doPrediction = true;
		stl::free_container(pPipeUser->m_State.predictedCharacterStates.states);
  }
  else
  {
    reachedEnd = true;
  }

  return ExecutePostamble(pPipeUser, reachedEnd, fullUpdate, false);
}

//===================================================================
// ExecuteLanding
//===================================================================
bool COPTrace::ExecuteLanding(CPipeUser* pPipeUser, const Vec3 &pathEnd)
{
  m_inhibitPathRegen = true;
	float normalSpeed, minSpeed, maxSpeed;
	pPipeUser->GetMovementSpeedRange(pPipeUser->m_State.fMovementUrgency, false, normalSpeed, minSpeed, maxSpeed);
	//  float slowSpeed = pPipeUser->GetManeuverMovementSpeed();
  Vec3 opPos = pPipeUser->GetPhysicsPos();

  Vec3 horMoveDir = pathEnd - opPos;
  horMoveDir.z = 0.0f;
  float error = horMoveDir.NormalizeSafe();

  Limit(error, 0.0f, 1.0f);
	float horSpeed = 0.3f * minSpeed * error;
  float verSpeed = 1.0f;

  pPipeUser->m_State.vMoveDir = horSpeed * horMoveDir - Vec3(0, 0, verSpeed);
  pPipeUser->m_State.vMoveDir.NormalizeSafe();
  pPipeUser->m_State.fDesiredSpeed = sqrtf(square(horSpeed) + square(verSpeed));

  if (pPipeUser->m_State.fMovementUrgency < 0.0f)
    pPipeUser->m_State.vMoveDir *= -1.0f;

  // set look dir
  if (m_landingDir.IsZero())
  {

    if (gAIEnv.CVars.DebugPathFinding)
      AILogAlways("COPTrace::ExecuteLanding starting final landing %s", GetNameSafe(pPipeUser));

    m_landingDir = pPipeUser->GetMoveDir();
    m_landingDir.z = 0.0f;
    m_landingDir.NormalizeSafe(Vec3Constants<float>::fVec3_OneX);
  }
  Vec3 navTargetPos = opPos + 100.0f * m_landingDir;
	m_refNavTarget->SetPos(navTargetPos);

  if (!pPipeUser->m_bLooseAttention)
  {
		m_looseAttentionId= pPipeUser->SetLooseAttentionTarget(m_refNavTarget);
  }

  // 
  pe_status_collisions stat;
  stat.pHistory = 0;
  int collisions = pPipeUser->GetPhysics()->GetStatus(&stat);
  if (collisions > 0)
    return true;
  else 
    return false;
}

//===================================================================
// DebugDraw
//===================================================================
void COPTrace::DebugDraw(CPipeUser* pPipeUser) const
{
	if (IsPathRegenerationInhibited())
	{
		CDebugDrawContext dc;
		dc->Draw3dLabel(pPipeUser->GetPhysicsPos(), 1.5f, "PATH LOCKED\n%s %s",
			m_inhibitPathRegen ? "Inhibit" : "", m_passingStraightNavSO ? "NavSO" : "");
	}
}

void COPTrace::ExecuteTraceDebugDraw(CPipeUser* pPipeUser)
{
	if (gAIEnv.CVars.DebugPathFinding)
	{
		if (IAIDebugRenderer* pRenderer = gAIEnv.pAIDebugRenderer)
		{
			if (m_bWaitingForPathResult)
			{
				pRenderer->DrawSphere(pPipeUser->GetPos() + Vec3(0, 0, 1.f), .5f, ColorB(255, 255, 0));
			}

			if (m_bWaitingForBusySmartObject)
			{
				pRenderer->DrawSphere(pPipeUser->GetPos() + Vec3(0, 0, 1.2f), .5f, ColorB(255, 0, 0));
			}
		}
	}
}

bool COPTrace::HandleAnimationPhase(CPipeUser* pPipeUser, bool bFullUpdate, bool* pbForceRegeneratePath, bool* pbExactPositioning, bool* pbTraceFinished)
{
	SOBJECTSTATE& pipeUserState = pPipeUser->m_State;

	switch (pipeUserState.curActorTargetPhase)
	{
	case eATP_Error:
		{
			if (m_actorTargetRequester == eTATR_None)
			{
				m_actorTargetRequester = m_pendingActorTargetRequester;
				m_pendingActorTargetRequester = eTATR_None;
			}
			
			switch (m_actorTargetRequester)	
			{
			case eTATR_EndOfPath:

				if (gAIEnv.CVars.DebugPathFinding)
					AILogAlways("COPTrace::ExecuteTrace resetting since error occurred during exact positioning %s", GetNameSafe(pPipeUser));
				
				if (bFullUpdate)
				{
					Reset(pPipeUser);
				}
				else
				{
					// TODO: Handle better the error case!
					StopMovement(pPipeUser);
					m_bBlock_ExecuteTrace_untilFullUpdateThenReset = true;
				}
				*pbTraceFinished = true;
				return false;

			case eTATR_NavSO:
				// Exact positioning has been failed at navigation smart object, regenerate path.
				*pbForceRegeneratePath = true;
				m_inhibitPathRegen = false;
				break;
			}
			
			m_actorTargetRequester = eTATR_None;
			m_pendingActorTargetRequester = eTATR_None;
		}
		// END eATP_Error
		break;
	
	case eATP_Waiting:
		*pbExactPositioning = true;
		break;
	
	case eATP_Playing:
		// While playing, keep the trace & prediction running.
		*pbExactPositioning = true;

		// Resurrect any remaining path section while we're playing the animation.
		// This allows path following to continue based on the next path's start position.
		pPipeUser->m_Path.ResurrectRemainingPath();
		break;
		
	case eATP_Starting:
	case eATP_Started:
		*pbExactPositioning = true;

		if (m_pendingActorTargetRequester != eTATR_None)
		{
			m_actorTargetRequester = m_pendingActorTargetRequester;
			m_pendingActorTargetRequester = eTATR_None;
		}
		
		if (m_stopOnAnimationStart && m_actorTargetRequester == eTATR_EndOfPath)
		{
			if (bFullUpdate)
			{
				Reset(pPipeUser);
			}
			else
			{
				StopMovement(pPipeUser);
				m_bBlock_ExecuteTrace_untilFullUpdateThenReset = true;
			}
			*pbTraceFinished = true;
			return false;
		}
		break;
		
	case eATP_Finished:
	case eATP_StartedAndFinished:
		switch (m_actorTargetRequester)
		{
		case eTATR_EndOfPath:
			m_actorTargetRequester = eTATR_None;
			// Exact positioning has been finished at the end of the path, the trace is completed.

			if (gAIEnv.CVars.DebugPathFinding)
				AILogAlways("COPTrace::ExecuteTrace resetting since exact position reached/animation finished %s", GetNameSafe(pPipeUser));

			if (bFullUpdate)
			{
				Reset(pPipeUser);
			}
			else
			{
				StopMovement(pPipeUser);
				m_bBlock_ExecuteTrace_untilFullUpdateThenReset = true;
			}
			*pbTraceFinished = true;
			return false;
			
		case eTATR_NavSO:
			{
				// Exact positioning and animation has been finished at navigation smart object, resurrect path.
				pPipeUser->m_State.fDistanceToPathEnd = pPipeUser->m_Path.GetDiscardedPathLength();
				pPipeUser->m_Path.ResurrectRemainingPath();
				
				if (gAIEnv.CVars.useMNM)
					pPipeUser->m_Path.PrepareNavigationalSmartObjectsForMNM(pPipeUser);
				else
					pPipeUser->m_Path.PrepareNavigationalSmartObjects(pPipeUser);
				
				pPipeUser->AdjustPath();
				*pbForceRegeneratePath = false;

				m_actorTargetRequester = eTATR_None;
				m_prevFrameStartTime.SetValue(0ll);

				// Update distance traveled during the exact-pos anim.
				Vec3 opPos = pPipeUser->GetPhysicsPos();
				m_fTravelDist += !pPipeUser->IsUsing3DNavigation() ? 
							Distance::Point_Point2D(opPos, m_lastPosition) : Distance::Point_Point(opPos, m_lastPosition);
							m_lastPosition = opPos;

				m_bWaitingForPathResult = false;
				m_inhibitPathRegen = true;
			}
			break;

		default:
			// A pending exact positioning request, maybe from previously interrupted trace.
			// Regenerate path, since the current path may be bogus because it was generated
			// before the animation had finished.
			*pbForceRegeneratePath = true;
			m_actorTargetRequester = eTATR_None;
			m_bWaitingForPathResult = true;
			m_bWaitingForBusySmartObject = false;
			m_inhibitPathRegen = false;
		}
		// END eATP_Finished & eATP_StartedAndFinished:
		break;
	}
	
	return true;
}


bool COPTrace::HandlePathResult(CPipeUser* pPipeUser, bool* pbReturnValue)
{
	switch (pPipeUser->m_nPathDecision)
	{
	case PATHFINDER_PATHFOUND:
		// Path found, continue.
		m_bWaitingForPathResult = false;
		return true;

	case PATHFINDER_NOPATH:
		// Could not find path, fail.
		m_bWaitingForPathResult = false;
		m_bBlock_ExecuteTrace_untilFullUpdateThenReset = true;
		*pbReturnValue = true;
		return false;

	default:
		// Wait for the path finder result, disable movement and wait.
		StopMovement(pPipeUser);
		*pbReturnValue = false;
		return false;
	}
}


bool COPTrace::IsVehicleAndDriverIsFallen(CPipeUser* pPipeUser)
{
	if (pPipeUser->GetType() == AIOBJECT_VEHICLE)
	{
		// Check if the vehicle driver is tranq'd and do not move if it is.
		if (EntityId driverId = pPipeUser->GetProxy()->GetLinkedDriverEntityId())
		{
			if (IEntity* pDriverEntity = gEnv->pEntitySystem->GetEntity(driverId))
			{
				if (IAIObject* pDriverAI = pDriverEntity->GetAI())
				{
					if (CAIActor* pDriverActor = pDriverAI->CastToCAIActor())
					{
						if (IAIActorProxy* pDriverProxy = pDriverActor->GetProxy())
						{
							if (pDriverProxy->GetActorIsFallen())
							{
								// The driver is unable to drive, do not drive.
								StopMovement(pPipeUser);
								return true; 
							}
						}
					}
				}
			}
		}
	}

	return false;
}


void COPTrace::RegeneratePath(CPipeUser* pPipeUser, bool* pbForceRegeneratePath)
{
	if (gAIEnv.CVars.useMNM == 0)
	{
		// only regenerate if there isn't a new path that's about to come our way anyway
		if (!gAIEnv.pPathfinder->IsFindingPathFor(pPipeUser))
		{
			// Store the request params in the path
			const SNavPathParams& params = pPipeUser->m_Path.GetParams();
			int origPathDecision = pPipeUser->m_nPathDecision;

			if (gAIEnv.CVars.DebugPathFinding)
			{
				AILogAlways("COPTrace::ExecuteTrace %s regenerating path to (%5.2f, %5.2f, %5.2f) (origPathDecision = %d)",
					GetNameSafe(pPipeUser), params.end.x, params.end.y, params.end.z, origPathDecision);
			}

			float fEndDistance = params.endDistance;
			if (fEndDistance < 0.f)
				fEndDistance = min(-0.01f, fEndDistance + m_fTravelDist);

			if (params.isDirectional)
			{
				float fSearchRange = Distance::Point_Point(params.end, pPipeUser->GetPhysicsPos());
				pPipeUser->RequestPathInDirection(params.end, fSearchRange, pPipeUser->m_refPathFindTarget, fEndDistance);
			}
			else
			{
				float fEndTolerance = (m_bForceReturnPartialPath || m_fEndAccuracy < 0.f)
					? std::numeric_limits<float>::max()
					: m_fEndAccuracy;

				CAIObject* pPathFindTarget = pPipeUser->m_refPathFindTarget.GetAIObject();
				pPipeUser->RequestPathTo(params.end, params.endDir, params.allowDangerousDestination, params.nForceBuildingID, fEndTolerance, fEndDistance, pPathFindTarget);
			}

			// Keep the travel dist per path to keep the end distance calculation correct over multiple regenerations.
			m_fTravelDist = 0.f;

			// If not forcing the regen because of navSO, pretend that the state of the path is still the same.
			if (!*pbForceRegeneratePath)
				pPipeUser->m_nPathDecision = origPathDecision;
		}

		m_fTotalTracingTime = 0.0f;
	}
	else
	{
		//AIWarning("COPTrace::RegeneratePath is not currently supported by the MNM system.");
	}
}


void COPTrace::StopMovement(CPipeUser* pPipeUser)
{
	SOBJECTSTATE& pipeUserState = pPipeUser->m_State;

	pipeUserState.vMoveDir.zero();
	pipeUserState.fDesiredSpeed = 0.f;
	stl::free_container(pipeUserState.predictedCharacterStates.states);
}


void COPTrace::TriggerExactPositioning(CPipeUser* pPipeUser, bool* pbForceRegeneratePath, bool* pbExactPositioning)
{
	SOBJECTSTATE& pipeUserState = pPipeUser->m_State;
	SAIActorTargetRequest& pipeUserActorTargetRequest = pipeUserState.actorTargetReq;

	switch (pipeUserState.curActorTargetPhase)
	{
	case eATP_None:
	{
		if (gAIEnv.CVars.DebugPathFinding)
		{
			SNavSOStates& pipeUserPendingNavSOStates = pPipeUser->m_pendingNavSOStates;
			if (!pipeUserPendingNavSOStates.IsEmpty())
			{
				IEntity* pEntity = gEnv->pEntitySystem->GetEntity(pipeUserPendingNavSOStates.objectEntId);
				if (pEntity)
				{
					AILogAlways("COPTrace::ExecuteTrace %s trying to use exact positioning while a navSO (entity=%s) is still active.",
						GetNameSafe(pPipeUser), pEntity->GetName());
				}
				else
				{
					AILogAlways("COPTrace::ExecuteTrace %s trying to use exact positioning while a navSO (entityId=%d) is still active.",
						GetNameSafe(pPipeUser), pipeUserPendingNavSOStates.objectEntId);
				}
			}
		}

		// RW - 16/06/2011
		// Prevents stick from never generating a new path after choosing a path
		// with a smartobject.
		pPipeUser->m_Path.GetParams().inhibitPathRegeneration = false;

		// Handle the exact positioning request
		PathPointDescriptor::SmartObjectNavDataPtr pSmartObjectNavData = pPipeUser->m_Path.GetLastPathPointAnimNavSOData();
		const PathPointDescriptor::SmartObjectMNMData*	pSmartObjectMNMData = pPipeUser->m_Path.GetLastPathPointMNNSOData();
		const bool useMNM = (gAIEnv.CVars.useMNM != 0);
		const bool smartObject = (pSmartObjectNavData && pSmartObjectNavData->fromIndex && pSmartObjectNavData->toIndex) && !useMNM;
		const bool smartObjectMNM = pSmartObjectMNMData ? (pSmartObjectMNMData->meshID && pSmartObjectMNMData->offMeshLinkID && useMNM) : false;

		if (smartObject)
		{
			CGraph* pGraph = gAIEnv.pGraph;

			// Navigational SO at the end of the current path.
			const GraphNode* pFromNode = pGraph->GetNode(pSmartObjectNavData->fromIndex);
			const GraphNode* pToNode = pGraph->GetNode(pSmartObjectNavData->toIndex);

			CSmartObjectManager* pSmartObjectManager = gAIEnv.pSmartObjectManager;

			const SSmartObjectNavData* pNavData = pFromNode->GetSmartObjectNavData();
			CSmartObject* pSmartObject = pNavData->pSmartObject;

			EntityId smartObjectId = pSmartObject->GetEntityId();

			// If the smart object isn't busy
			if (pSmartObjectManager->IsSmartObjectBusy( pSmartObject ))
			{
				// Attempt to wait for the SO to become free
				m_bWaitingForBusySmartObject = true;
			}
			else
			{
				const Vec3 smartObjectStart = pFromNode->GetPos();
				CAIActor* closestActor = pPipeUser->CastToCAIActor();

				// Find the closest of all actors trying to use this SmartObject
				{
					ActorLookUp& lookUp = *gAIEnv.pActorLookUp;
					size_t activeCount = lookUp.GetActiveCount();

					float distanceClosestSq = Distance::Point_PointSq(closestActor->GetPos(), smartObjectStart);

					for (size_t actorIndex = 0; actorIndex < activeCount; ++actorIndex)
					{
						const Vec3 position = lookUp.GetPosition(actorIndex);
						const float distanceSq = Distance::Point_PointSq(position, smartObjectStart);

						if (distanceSq < distanceClosestSq)
						{
							if (CPipeUser* closerActor = lookUp.GetActor<CPipeUser>(actorIndex))
							{
								if (closerActor->GetPendingSmartObjectID() == smartObjectId)
								{
									distanceClosestSq = distanceSq;

									closestActor = closerActor;
								}
							}
						}
					}
				}

				assert(closestActor);

				// Are we the closest candidate?
				if (closestActor && (closestActor == pPipeUser->CastToCAIActor()))
				{
					// Fill in the actor target request, and figure out the navSO method.
					if (pSmartObjectManager->PrepareNavigateSmartObject(pPipeUser, pFromNode, pToNode) && pPipeUser->m_eNavSOMethod != nSOmNone)
					{
						pipeUserActorTargetRequest.id = ++pPipeUser->m_actorTargetReqId;
						pipeUserActorTargetRequest.bLowerPrecision = true;

						m_pendingActorTargetRequester = eTATR_NavSO;

						// In case we hit here because the path following has finished, keep the trace alive.
						*pbExactPositioning = true;

						// Enforce to use the current path.
						pPipeUser->m_Path.GetParams().inhibitPathRegeneration = true;
						gAIEnv.pPathfinder->CancelAnyPathsFor(pPipeUser);

#ifdef _DEBUG
						// TODO: these are debug variables, should be perhaps initialised somewhere else.
						pPipeUser->m_DEBUGCanTargetPointBeReached.clear();
						pPipeUser->m_DEBUGUseTargetPointRequest.zero();
#endif

						m_bWaitingForBusySmartObject = false;
					}
					else	// Can't navigate using this SO for some reason
					{
						// Failed to use the navSO. Instead of resetting the goalop, set the state
						// to error, to prevent the link being reused.
						*pbForceRegeneratePath = true;
						pipeUserActorTargetRequest.Reset();
						m_actorTargetRequester = eTATR_None;
						m_pendingActorTargetRequester = eTATR_None;

						pPipeUser->InvalidateSOLink(pSmartObject, pNavData->pHelper, pGraph->GetNode(pSmartObjectNavData->toIndex)->GetSmartObjectNavData()->pHelper);
					}
				}
				else // We are not the closest in line
				{
					// Attempt to wait for the SO to become free
					m_bWaitingForBusySmartObject = true;
				}
			}
		}
		else if (smartObjectMNM)
		{
			MNM::OffMeshNavigation& offMeshNavigation = gAIEnv.pNavigationSystem->GetOffMeshNavigationManager()->GetOffMeshNavigationForMesh(NavigationMeshID(pSmartObjectMNMData->meshID));
			MNM::OffMeshLink* pOffMeshLink = offMeshNavigation.GetObjectLinkInfo(pSmartObjectMNMData->offMeshLinkID);
			OffMeshLink_SmartObject* pSOLink = pOffMeshLink ? pOffMeshLink->CastTo<OffMeshLink_SmartObject>() : NULL;

			assert(pSOLink);
			if (pSOLink)
			{
				CSmartObjectManager* pSmartObjectManager = gAIEnv.pSmartObjectManager;
				CSmartObject* pSmartObject = pSOLink->m_pSmartObject;

				const EntityId smartObjectId = pSmartObject->GetEntityId();

				// If the smart object isn't busy
				if (pSmartObjectManager->IsSmartObjectBusy( pSmartObject ))
				{
					// Attempt to wait for the SO to become free
					m_bWaitingForBusySmartObject = true;
				}
				else
				{
					const Vec3 smartObjectStart = pSmartObject->GetHelperPos(pSOLink->m_pFromHelper);
					CAIActor* closestActor = pPipeUser->CastToCAIActor();

					// Find the closest of all actors trying to use this SmartObject
					{
						ActorLookUp& lookUp = *gAIEnv.pActorLookUp;
						size_t activeCount = lookUp.GetActiveCount();

						float distanceClosestSq = Distance::Point_PointSq(closestActor->GetPos(), smartObjectStart);

						for (size_t actorIndex = 0; actorIndex < activeCount; ++actorIndex)
						{
							const Vec3 position = lookUp.GetPosition(actorIndex);
							const float distanceSq = Distance::Point_PointSq(position, smartObjectStart);

							if (distanceSq < distanceClosestSq)
							{
								if (CPipeUser* closerActor = lookUp.GetActor<CPipeUser>(actorIndex))
								{
									if (closerActor->GetPendingSmartObjectID() == smartObjectId)
									{
										distanceClosestSq = distanceSq;

										closestActor = closerActor;
									}
								}
							}
						}
					}

					assert(closestActor);

					// Are we the closest candidate?
					if (closestActor && (closestActor == pPipeUser->CastToCAIActor()))
					{
						// Fill in the actor target request, and figure out the navSO method.
						if (pSmartObjectManager->PrepareNavigateSmartObject(pPipeUser, pSmartObject, pSOLink->m_pSmartObjectClass, pSOLink->m_pFromHelper, pSOLink->m_pToHelper) && pPipeUser->m_eNavSOMethod != nSOmNone)
						{
							pipeUserActorTargetRequest.id = ++pPipeUser->m_actorTargetReqId;
							pipeUserActorTargetRequest.bLowerPrecision = true;

							m_pendingActorTargetRequester = eTATR_NavSO;

							// In case we hit here because the path following has finished, keep the trace alive.
							*pbExactPositioning = true;

							// Enforce to use the current path.
							pPipeUser->m_Path.GetParams().inhibitPathRegeneration = true;
							pPipeUser->CancelRequestedPath(false);

#ifdef _DEBUG
							// TODO: these are debug variables, should be perhaps initialised somewhere else.
							pPipeUser->m_DEBUGCanTargetPointBeReached.clear();
							pPipeUser->m_DEBUGUseTargetPointRequest.zero();
#endif

							m_bWaitingForBusySmartObject = false;
						}
						else	// Can't navigate using this SO for some reason
						{
							// Failed to use the navSO. Instead of resetting the goalop, set the state
							// to error, to prevent the link being reused.
							*pbForceRegeneratePath = true;
							pipeUserActorTargetRequest.Reset();
							m_actorTargetRequester = eTATR_None;
							m_pendingActorTargetRequester = eTATR_None;

							//Should this be used for MNM case?
							pPipeUser->InvalidateSOLink(pSmartObject, pSOLink->m_pFromHelper, pSOLink->m_pToHelper);
						}
					}
					else // We are not the closest in line
					{
						// Attempt to wait for the SO to become free
						m_bWaitingForBusySmartObject = true;
					}
				}
			}
		}
		// No SO nav data
		else if (const SAIActorTargetRequest* pActiveActorTargetRequest = pPipeUser->GetActiveActorTargetRequest())
		{
			// Actor target requested at the end of the path.
			pipeUserActorTargetRequest = *pActiveActorTargetRequest;
			pipeUserActorTargetRequest.id = ++pPipeUser->m_actorTargetReqId;
			pipeUserActorTargetRequest.bLowerPrecision = false;
			m_pendingActorTargetRequester = eTATR_EndOfPath;
			
			// In case we hit here because the path following has finished, keep the trace alive.
			*pbExactPositioning = true;

			// Enforce to use the current path.
			pPipeUser->m_Path.GetParams().inhibitPathRegeneration = true;
			gAIEnv.pPathfinder->CancelAnyPathsFor(pPipeUser);

#ifdef _DEBUG
			// TODO: these are debug variables, should be perhaps initialised somewhere else.
			pPipeUser->m_DEBUGCanTargetPointBeReached.clear();
			pPipeUser->m_DEBUGUseTargetPointRequest.zero();
#endif
		}
		break;
	}
	
	case eATP_Error:
		break;
		
	default:
		// The exact positioning is in motion but not yet playing, keep the trace alive.
		*pbExactPositioning = true;
	}
}
